#include "serial.hpp"
#include "serial_to_stm32/shoot_status_t.h"
#include "serial_to_stm32/incoming_msg.h"
#include "serial_to_stm32/outgoing_msg.h"
#include <cstdint>
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h> 
#include <cmath>
#include <tf2_ros/message_filter.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <vector>

#define PI 3.1415926536

float loop_limit(float input);

uint8_t last_auto_status;
uint8_t now_auto_status;

int main(int argc, char *argv[]) {
	ros::init(argc, argv, "mimic_node_v2");
	ros::NodeHandle nh;

	std::string device_path = "/dev/ttyACM0";
	ros::param::get("~device_path", device_path);
	DemoBoard board(device_path);

    serial_to_stm32::incoming_msg gimbal_feedback_data;

    //创建订阅者，订阅名为 /gimbal_control 的topic
    ros::Subscriber gimbal_control_sub = nh.subscribe<serial_to_stm32::outgoing_msg>(
        "/gimbal_control", 1, [&](const serial_to_stm32::outgoing_msg::ConstPtr& msg){ // 订阅回调函数
            outgoing_msg serial_gimbal_control_data;
            float temp_yaw;
            float temp_pitch;
            float temp_frequency;
            if(last_auto_status == 0 && now_auto_status == 1){
                //ROS_INFO("swith into auto mode");
                temp_yaw = gimbal_feedback_data.gimbal_yaw;
                temp_pitch = gimbal_feedback_data.gimbal_pitch;
            }
            else if(last_auto_status == 1 && now_auto_status == 0){
                //ROS_INFO("swith out auto mode");
            }
            else{
                temp_yaw = msg->gimbal_yaw;
                temp_pitch = msg->gimbal_pitch;
            }
            temp_yaw = loop_limit(temp_yaw);
            temp_pitch = (temp_pitch >= 0.49) ? (0.49) : ((temp_pitch <= -0.67) ? -0.67 : temp_pitch);
            temp_frequency = (msg->shoot_status.frequency >= 15) ? (15) : (( msg->shoot_status.frequency <= 0.2) ? 0.2 : msg->shoot_status.frequency);
            if(msg->shoot_status.state != 0 && msg->shoot_status.state != 1 && msg->shoot_status.state != 2)
                serial_gimbal_control_data.shoot_status.state = 0;
            else
                serial_gimbal_control_data.shoot_status.state = msg->shoot_status.state;
            serial_gimbal_control_data.gimbal_yaw = temp_yaw * 1000000;
            serial_gimbal_control_data.gimbal_pitch = temp_pitch * 1000000;
            serial_gimbal_control_data.shoot_status.bullets_count = msg->shoot_status.bullets_count;
            serial_gimbal_control_data.shoot_status.frequency = temp_frequency * 1000000;
            /*ROS_INFO("receive gimbal_control:\nyaw: %f,  pitch: %f,  shoot_status: %d|%d|%d\r\n",
                    msg->gimbal_yaw, msg->gimbal_pitch,
                    msg->shoot_status.state, msg->shoot_status.bullets_count, msg->shoot_status.frequency);*/
            //发送数据
            board.send_message(serial_gimbal_control_data);
            /*ROS_INFO("send gimbal control:\nyaw: %f,  pitch: %f,  shoot_status: %d|%d|%f\r\n",
                    temp_yaw, temp_pitch,
                    serial_gimbal_control_data.shoot_status.state,serial_gimbal_control_data.shoot_status.bullets_count,temp_frequency);*/
        });
  
    //创建发布者，发布名为 /gimbal_feedback 的topic
    ros::Publisher gimbal_feedback_pub = nh.advertise<serial_to_stm32::incoming_msg>("gimbal_feedback", 1);

    tf2_ros::TransformBroadcaster br;

	ros::Rate rate(1000);
	while (ros::ok()) {
		auto msg_in = board.poll_message();
		if (msg_in.has_value()) {
            last_auto_status = now_auto_status;
            gimbal_feedback_data.gimbal_yaw = msg_in->gimbal_yaw / 1000000.0;
            gimbal_feedback_data.gimbal_pitch = msg_in->gimbal_pitch / 1000000.0;
            gimbal_feedback_data.shoot_status.state = msg_in->shoot_status.state;
            gimbal_feedback_data.shoot_status.bullets_count = msg_in->shoot_status.bullets_count;
            gimbal_feedback_data.shoot_status.frequency = msg_in->shoot_status.frequency / 1000000.0;
            gimbal_feedback_data.auto_status = msg_in->auto_status;
            now_auto_status = gimbal_feedback_data.auto_status;
			gimbal_feedback_pub.publish(gimbal_feedback_data);
            /* ROS_INFO("receive gimbal_feedback:\nyaw: %f,  pitch: %f,  shoot_status: %d|%d|%d,  auto_status: %d\r\n",
                    gimbal_feedback_data.gimbal_yaw, gimbal_feedback_data.gimbal_pitch, 
                    gimbal_feedback_data.shoot_status.state,gimbal_feedback_data.shoot_status.bullets_count,gimbal_feedback_data.shoot_status.frequency,
                    gimbal_feedback_data.auto_status);*/
           
            geometry_msgs::TransformStamped transformStamped;
            transformStamped.header.stamp = ros::Time::now();
            transformStamped.header.frame_id = "gimbal_link";
            transformStamped.child_frame_id = "yaw_base_link";
            transformStamped.transform.translation.x=0;
            transformStamped.transform.translation.y=0;
            transformStamped.transform.translation.z=0.09085;
            tf2::Quaternion q;
            q.setRPY(0,0,gimbal_feedback_data.gimbal_yaw);
            transformStamped.transform.rotation.x=q.x();
            transformStamped.transform.rotation.y=q.y();
            transformStamped.transform.rotation.z=q.z();
            transformStamped.transform.rotation.w=q.w();
            br.sendTransform(transformStamped);
		}
        ros::spinOnce();
		rate.sleep();
	}
}

float loop_limit(float input){
  while( input <= -PI ){
    input += 2 * PI;
  }
  while(input >= PI){
    input -= 2 * PI;
  }
  return input;
}